#include <tracker/tracker.h>


/**
 *  Tracker constructor.
 **/
Tracker::Tracker () :
	filter_(1),
	current_angle(0),
	current_x(0),
	current_y(0)
{
	ros::NodeHandle n;
	movement_subscriber 	= n.subscribe("/motor_controller/movement", 1000, & Tracker::movement_callback, this);
	location_server 	= n.advertiseService("/slam/location", 	   & Tracker::location_service, this);
	phase_subscriber_ = n.subscribe("/SLAM/phase", 100, &Tracker::switch_phase_callback, this);

	if (!n.getParam("/motor/wheel_base", wheel_base)) {
		ROS_ERROR("Tracker failed to get param /motor/wheel_base");
	}

	ROS_INFO("Tracker ready.\n");
	ros::spin();
}

void Tracker::switch_phase_callback(const SLAM::SwitchPhase::ConstPtr& msg) {
	if (msg->phase == 1 || msg->phase == 3) {
		current_x = current_y = current_angle = 0.0f;
	}
}

/**
 *  The mazin-virtual Tracker class is designed to track and visualize the actual progress
 *  of a robot, based on encoder readings.
 */
int main (int argc, char **argv)
{
  	ros::init(argc, argv, "tracker");
	new Tracker();
	return 0;
}
